#include <base/log.h>
#include <base/debug.h>
#include <base/irq.h>
#include <arch/gicv2.h>
#include <arch/context.h>
#include <arch/esr.h>
#include <arch/sysregs.h>
#include <base/thread.h>

static const char * const bad_mode_handler[] = {
	"Sync Abort",
	"IRQ",
	"FIQ",
	"Error"
};

static const char *esr_class_str[] = {
	[0 ... ESR_ELx_EC_MAX]		= "UNRECOGNIZED EC",
	[ESR_ELx_EC_UNKNOWN]		= "Unknown/Uncategorized",
	[ESR_ELx_EC_WFx]		= "WFI/WFE",
	[ESR_ELx_EC_CP15_32]		= "CP15 MCR/MRC",
	[ESR_ELx_EC_CP15_64]		= "CP15 MCRR/MRRC",
	[ESR_ELx_EC_CP14_MR]		= "CP14 MCR/MRC",
	[ESR_ELx_EC_CP14_LS]		= "CP14 LDC/STC",
	[ESR_ELx_EC_FP_ASIMD]		= "ASIMD",
	[ESR_ELx_EC_CP10_ID]		= "CP10 MRC/VMRS",
	[ESR_ELx_EC_CP14_64]		= "CP14 MCRR/MRRC",
	[ESR_ELx_EC_ILL]		= "PSTATE.IL",
	[ESR_ELx_EC_SVC32]		= "SVC (AArch32)",
	[ESR_ELx_EC_HVC32]		= "HVC (AArch32)",
	[ESR_ELx_EC_SMC32]		= "SMC (AArch32)",
	[ESR_ELx_EC_SVC64]		= "SVC (AArch64)",
	[ESR_ELx_EC_HVC64]		= "HVC (AArch64)",
	[ESR_ELx_EC_SMC64]		= "SMC (AArch64)",
	[ESR_ELx_EC_SYS64]		= "MSR/MRS (AArch64)",
	[ESR_ELx_EC_IMP_DEF]		= "EL3 IMP DEF",
	[ESR_ELx_EC_IABT_LOW]		= "IABT (lower EL)",
	[ESR_ELx_EC_IABT_CUR]		= "IABT (current EL)",
	[ESR_ELx_EC_PC_ALIGN]		= "PC Alignment",
	[ESR_ELx_EC_DABT_LOW]		= "DABT (lower EL)",
	[ESR_ELx_EC_DABT_CUR]		= "DABT (current EL)",
	[ESR_ELx_EC_SP_ALIGN]		= "SP Alignment",
	[ESR_ELx_EC_FP_EXC32]		= "FP (AArch32)",
	[ESR_ELx_EC_FP_EXC64]		= "FP (AArch64)",
	[ESR_ELx_EC_SERROR]		= "SError",
	[ESR_ELx_EC_BREAKPT_LOW]	= "Breakpoint (lower EL)",
	[ESR_ELx_EC_BREAKPT_CUR]	= "Breakpoint (current EL)",
	[ESR_ELx_EC_SOFTSTP_LOW]	= "Software Step (lower EL)",
	[ESR_ELx_EC_SOFTSTP_CUR]	= "Software Step (current EL)",
	[ESR_ELx_EC_WATCHPT_LOW]	= "Watchpoint (lower EL)",
	[ESR_ELx_EC_WATCHPT_CUR]	= "Watchpoint (current EL)",
	[ESR_ELx_EC_BKPT32]		= "BKPT (AArch32)",
	[ESR_ELx_EC_VECTOR32]		= "Vector catch (AArch32)",
	[ESR_ELx_EC_BRK64]		= "BRK (AArch64)",
};

static const char *esr_get_class_string(unsigned int esr)
{
	return esr_class_str[esr >> ESR_ELx_EC_SHIFT];
}

void show_frame(NX_HalTrapFrame * regs)
{
	NX_Printf("Execption:\r\n");
    NX_Printf("X00:0x%16.16p X01:0x%16.16p X02:0x%16.16p X03:0x%16.16p\r\n", (void *)regs->x0, (void *)regs->x1, (void *)regs->x2, (void *)regs->x3);
    NX_Printf("X04:0x%16.16p X05:0x%16.16p X06:0x%16.16p X07:0x%16.16p\r\n", (void *)regs->x4, (void *)regs->x5, (void *)regs->x6, (void *)regs->x7);
    NX_Printf("X08:0x%16.16p X09:0x%16.16p X10:0x%16.16p X11:0x%16.16p\r\n", (void *)regs->x8, (void *)regs->x9, (void *)regs->x10, (void *)regs->x11);
    NX_Printf("X12:0x%16.16p X13:0x%16.16p X14:0x%16.16p X15:0x%16.16p\r\n", (void *)regs->x12, (void *)regs->x13, (void *)regs->x14, (void *)regs->x15);
    NX_Printf("X16:0x%16.16p X17:0x%16.16p X18:0x%16.16p X19:0x%16.16p\r\n", (void *)regs->x16, (void *)regs->x17, (void *)regs->x18, (void *)regs->x19);
    NX_Printf("X20:0x%16.16p X21:0x%16.16p X22:0x%16.16p X23:0x%16.16p\r\n", (void *)regs->x20, (void *)regs->x21, (void *)regs->x22, (void *)regs->x23);
    NX_Printf("X24:0x%16.16p X25:0x%16.16p X26:0x%16.16p X27:0x%16.16p\r\n", (void *)regs->x24, (void *)regs->x25, (void *)regs->x26, (void *)regs->x27);
    NX_Printf("X28:0x%16.16p X29:0x%16.16p X30:0x%16.16p\r\n", (void *)regs->x28, (void *)regs->x29, (void *)regs->x30);
    NX_Printf("SP_EL0:0x%16.16p\r\n", (void *)regs->sp_el0);
    NX_Printf("SPSR  :0x%16.16p\r\n", (void *)regs->cpsr);
    NX_Printf("EPC   :0x%16.16p\r\n", (void *)regs->pc);
}


void bad_mode(NX_HalTrapFrame * frame, int reason, unsigned int esr)
{
    NX_Printf("Bad mode for %s handler detected, far:0x%x esr:0x%x  - %s\r\n",
			bad_mode_handler[reason], read_sysreg(far_el1),
			esr, esr_get_class_string(esr));

	show_frame(frame);
    NX_Printf("current: %s\r\n", NX_ThreadSelf()->name);

	NX_PANIC("bad mode\r\n");
}

NX_IMPORT NX_Error NX_HalHandlePageFault(NX_Thread * thread, NX_HalTrapFrame * frame, NX_Addr faultAddr);
static void data_abort(unsigned long far, unsigned long iss)
{
    NX_Printf("fault addr = 0x%016lx\r\n", far);
    if (iss & 0x40)
    {
        NX_Printf("abort caused by write instruction\r\n");
    }
    else
    {
        NX_Printf("abort caused by read instruction\r\n");
    }
    switch (iss & 0x3f)
    {
    case 0b000000:
        NX_Printf("Address size fault, zeroth level of translation or translation table base register\r\n");
        break;

    case 0b000001:
        NX_Printf("Address size fault, first level\r\n");
        break;

    case 0b000010:
        NX_Printf("Address size fault, second level\r\n");
        break;

    case 0b000011:
        NX_Printf("Address size fault, third level\r\n");
        break;

    case 0b000100:
        NX_Printf("Translation fault, zeroth level\r\n");
        break;

    case 0b000101:
        NX_Printf("Translation fault, first level\r\n");
        break;

    case 0b000110:
        NX_Printf("Translation fault, second level\r\n");
        break;

    case 0b000111:
        NX_Printf("Translation fault, third level\r\n");
        break;

    case 0b001001:
        NX_Printf("Access flag fault, first level\r\n");
        break;

    case 0b001010:
        NX_Printf("Access flag fault, second level\r\n");
        break;

    case 0b001011:
        NX_Printf("Access flag fault, third level\r\n");
        break;

    case 0b001101:
        NX_Printf("Permission fault, first level\r\n");
        break;

    case 0b001110:
        NX_Printf("Permission fault, second level\r\n");
        break;

    case 0b001111:
        NX_Printf("Permission fault, third level\r\n");
        break;

    case 0b010000:
        NX_Printf("Synchronous external abort, not on translation table walk\r\n");
        break;

    case 0b011000:
        NX_Printf("Synchronous parity or ECC error on memory access, not on translation table walk\r\n");
        break;

    case 0b010100:
        NX_Printf("Synchronous external abort on translation table walk, zeroth level\r\n");
        break;

    case 0b010101:
        NX_Printf("Synchronous external abort on translation table walk, first level\r\n");
        break;

    case 0b010110:
        NX_Printf("Synchronous external abort on translation table walk, second level\r\n");
        break;

    case 0b010111:
        NX_Printf("Synchronous external abort on translation table walk, third level\r\n");
        break;

    case 0b011100:
        NX_Printf("Synchronous parity or ECC error on memory access on translation table walk, zeroth level\r\n");
        break;

    case 0b011101:
        NX_Printf("Synchronous parity or ECC error on memory access on translation table walk, first level\r\n");
        break;

    case 0b011110:
        NX_Printf("Synchronous parity or ECC error on memory access on translation table walk, second level\r\n");
        break;

    case 0b011111:
        NX_Printf("Synchronous parity or ECC error on memory access on translation table walk, third level\r\n");
        break;

    case 0b100001:
        NX_Printf("Alignment fault\r\n");
        break;

    case 0b110000:
        NX_Printf("TLB conflict abort\r\n");
        break;

    case 0b110100:
        NX_Printf("IMPLEMENTATION DEFINED fault (Lockdown fault)\r\n");
        break;

    case 0b110101:
        NX_Printf("IMPLEMENTATION DEFINED fault (Unsupported Exclusive access fault)\r\n");
        break;

    case 0b111101:
        NX_Printf("Section Domain Fault, used only for faults reported in the PAR_EL1\r\n");
        break;

    case 0b111110:
        NX_Printf("Page Domain Fault, used only for faults reported in the PAR_EL1\r\n");
        break;

    default:
        NX_Printf("unknow abort\r\n");
        break;
    }
}

void process_exception(NX_HalTrapFrame *frame, unsigned long esr, unsigned long epc)
{
    NX_U8 ec;
    NX_U32 iss;
    unsigned long fault_addr;
    NX_Printf("\nexception info:\r\n");
    ec = (unsigned char)((esr >> 26) & 0x3fU);
    iss = (unsigned int)(esr & 0x00ffffffU);
    NX_Printf("esr.EC :0x%02x\r\n", ec);
    NX_Printf("esr.IL :0x%02x\r\n", (unsigned char)((esr >> 25) & 0x01U));
    NX_Printf("esr.ISS:0x%08x\r\n", iss);
    NX_Printf("epc    :0x%016p\r\n", (void *)epc);
    switch (ec)
    {
    case 0x00:
        NX_Printf("Exceptions with an unknow reason\r\n");
        break;

    case 0x01:
        NX_Printf("Exceptions from an WFI or WFE instruction\r\n");
        break;

    case 0x03:
        NX_Printf("Exceptions from an MCR or MRC access to CP15 from AArch32\r\n");
        break;

    case 0x04:
        NX_Printf("Exceptions from an MCRR or MRRC access to CP15 from AArch32\r\n");
        break;

    case 0x05:
        NX_Printf("Exceptions from an MCR or MRC access to CP14 from AArch32\r\n");
        break;

    case 0x06:
        NX_Printf("Exceptions from an LDC or STC access to CP14 from AArch32\r\n");
        break;

    case 0x07:
        NX_Printf("Exceptions from Access to Advanced SIMD or floating-point registers\r\n");
        break;

    case 0x08:
        NX_Printf("Exceptions from an MRC (or VMRS) access to CP10 from AArch32\r\n");
        break;

    case 0x0c:
        NX_Printf("Exceptions from an MCRR or MRRC access to CP14 from AArch32\r\n");
        break;

    case 0x0e:
        NX_Printf("Exceptions that occur because ther value of PSTATE.IL is 1\r\n");
        break;

    case 0x11:
        NX_Printf("SVC call from AArch32 state\r\n");
        break;

    case 0x15:
        NX_Printf("SVC call from AArch64 state\r\n");
        break;

    case 0x20:
        NX_Printf("Instruction abort from lower exception level\r\n");
        break;

    case 0x21:
        NX_Printf("Instruction abort from current exception level\r\n");
        break;

    case 0x22:
        NX_Printf("PC alignment fault\r\n");
        break;

    case ESR_ELx_EC_DABT_LOW:
        NX_Printf("Data abort from a lower Exception level\r\n");
        __asm__ volatile("mrs %0, far_el1":"=r"(fault_addr));
        data_abort(fault_addr, iss);
        break;

    case ESR_ELx_EC_DABT_CUR:
        NX_Printf("Data abort\r\n");
        __asm__ volatile("mrs %0, far_el1":"=r"(fault_addr));
        data_abort(fault_addr, iss);
        break;

    default:
        NX_Printf("Other error\r\n");
        break;
    }
}
NX_IMPORT void NX_HalProcessSyscallDispatch(NX_HalTrapFrame *frame);

void HandleException(NX_HalTrapFrame *frame)
{
	unsigned long esr;
    unsigned char ec;
    unsigned long fault_addr;

    asm volatile("mrs %0, esr_el1":"=r"(esr));
    ec = (unsigned char)((esr >> 26) & 0x3fU);

    if (ec == ESR_ELx_EC_SVC64) /* is 64bit syscall ? */
    {
		// NX_LOG_W("do syscall here! x8=%d", frame->x8);
        /* never return here */
        NX_HalProcessSyscallDispatch(frame);
		return;
    }
    
    if (ec == ESR_ELx_EC_DABT_LOW || ec == ESR_ELx_EC_DABT_CUR)
    {
        __asm__ volatile("mrs %0, far_el1":"=r"(fault_addr));
        if (NX_HalHandlePageFault(NX_ThreadSelf(), frame, fault_addr) == NX_EOK)
        {
            return;
        }
    }

    process_exception(frame, esr, frame->pc);
    show_frame(frame);
    NX_Printf("current: %s\r\n", NX_ThreadSelf()->name);

	NX_PANIC("EL0 Exception");
}

void TrapDispatch(NX_HalTrapFrame *frame)
{
	HandleException(frame);
    NX_IRQ_OverCheck(frame);
    NX_SignalCheck(frame);
    NX_IRQ_OverCheck2(frame);
}

void irq_handle(NX_HalTrapFrame * frame)
{
    int irqno;
    int max_irq;

	max_irq = gicv2_get_max_irq();
	NX_ASSERT(max_irq > 0);

	do {

		irqno = gicv2_get_current_irq();
		if (irqno > 15 && irqno < max_irq)
		{
            NX_IRQ_Enter();
			NX_IRQ_Handle(irqno);
			NX_IRQ_Exit();
            continue;
		}

		if (irqno < 16) {
			gicv2_eoi_irq(0, irqno);
			continue;
		}
		break;
	} while(1);

	NX_IRQ_OverCheck(frame);
    NX_SignalCheck(frame);
	NX_IRQ_OverCheck2(frame);
}
